#include <string.h>	
#include <unistd.h>
#include <iostream>
#include <fstream>
#include <sstream>
#include <sys/time.h>
#include "DroneState.h"
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/TwistStamped.h"
#include "mavros_msgs/State.h"
#include "mavros_msgs/SetMode.h" 
#include <mavros_msgs/CommandBool.h>
#include "geometry_msgs/Vector3Stamped.h"

using namespace std;
static vector<vector<double>> originPositionVec;
static mavros_msgs::State subMode;
static int drone_number = -1;
static int max_swarm_number = 0;
static string server_ip;
static int listeningPort = 0;
Json::Value subTopicConfig;
Json::Value pubTopicHeadConfig;
Json::Value pubControlConfig;
Json::Value emergencyLandingConfig;

void velocityCallback(const geometry_msgs::TwistStamped::ConstPtr& msg, DroneState* myMsg) {
	if (msg == nullptr) {
		return;
	}
	myMsg->velocity[0] = msg->twist.linear.x;
	myMsg->velocity[1] = msg->twist.linear.y;
	myMsg->velocity[2] = msg->twist.linear.z;
	return;
}

void poseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg, DroneState* myMsg) {
	if (msg == nullptr) {
		return;
	}
	//globalPoseSend
	myMsg->position[0] = msg->pose.position.x + originPositionVec[drone_number][0];
	myMsg->position[1] = msg->pose.position.y + originPositionVec[drone_number][1];
	myMsg->position[2] = msg->pose.position.z + + originPositionVec[drone_number][2];
	myMsg->orientation[0] = msg->pose.orientation.x;
	myMsg->orientation[1] = msg->pose.orientation.y;
	myMsg->orientation[2] = msg->pose.orientation.z;
	myMsg->orientation[3] = msg->pose.orientation.w;
	return;
}

void stateCallback(const mavros_msgs::State::ConstPtr& msg, DroneState* myMsg) {
	if (msg == nullptr) {
		return;
	}
	myMsg->mode = msg->mode;
	subMode = *msg;
	return;
}

void ownMsgCallback(const std_msgs::String::ConstPtr& msg, DroneState* myMsg) {
	if (msg == nullptr) {
		return;
	}
	myMsg->bridgeMessage = msg->data;
	return;
}

class ClientHelper {
public:
	ClientHelper(ros::NodeHandle& n, DroneState* msg, int max_swarm_number);
	void pubHelper(vector<DroneState>& recvMsg);
	void swarmMsgPubHelper(const std::string&);
	void pubHelper(ControlMsg& controlMsg);
public:
	ros::Subscriber ownMsgSub;
	ros::Subscriber subPose;
	ros::Subscriber subState;
	ros::Subscriber subVelocity;
	vector<ros::Publisher> pubPoseVec;
	vector<ros::Publisher> pubVelocityVec;
	vector<ros::Publisher> pubStateVec;
	ros::Publisher pubSwarmMsg;
	ros::NodeHandle& pn;
	ros::Publisher pubControlPositon;
	ros::ServiceClient pubControlMode;
	ros::ServiceClient pubArmControl;
	ros::Publisher pubAcc;
};

ClientHelper::ClientHelper(ros::NodeHandle& n, DroneState* msg, int max_swarm_number):pn{n} {
		std::cout<<"ClientHelper inistialize..."<<std::endl;
		ownMsgSub = n.subscribe<std_msgs::String>(subTopicConfig[drone_number]["subOwnMsg"].asString(), 100, boost::bind(&ownMsgCallback, _1, msg));
		subPose = n.subscribe<geometry_msgs::PoseStamped>(subTopicConfig[drone_number]["subMavrosPose"].asString(), 100, boost::bind(&poseCallback, _1, msg));
		subState = n.subscribe<mavros_msgs::State>(subTopicConfig[drone_number]["subMavrosState"].asString(), 100, boost::bind(&stateCallback, _1, msg));
		subVelocity = n.subscribe<geometry_msgs::TwistStamped>(subTopicConfig[drone_number]["subMavrosVelocity"].asString(), 100, boost::bind(&velocityCallback, _1, msg));
		
		pubControlPositon = n.advertise<geometry_msgs::PoseStamped>(pubControlConfig[drone_number]["pubControlPositon"].asString(), 10);
		pubAcc = n.advertise<geometry_msgs::Vector3Stamped>(pubControlConfig[drone_number]["pubAcc"].asString(), 10);
		pubControlMode = n.serviceClient<mavros_msgs::SetMode>(pubControlConfig[drone_number]["pubControlMode"].asString());
		pubArmControl = n.serviceClient<mavros_msgs::CommandBool>(pubControlConfig[drone_number]["pubArmControl"].asString());
			
		pubSwarmMsg = n.advertise<std_msgs::String>(pubTopicHeadConfig["pubSwarmMsg"].asString(), 10);
		for (int i = 0; i < max_swarm_number; ++i) {
			pubPoseVec.push_back(n.advertise<geometry_msgs::PoseStamped>(pubTopicHeadConfig["pubMavrosHead"].asString() + std::to_string(i) + "/global_position", 10));
			pubVelocityVec.push_back(n.advertise<geometry_msgs::TwistStamped>(pubTopicHeadConfig["pubMavrosHead"].asString() + std::to_string(i) + "/mavros/local_position/velocity_local", 10));
			pubStateVec.push_back(n.advertise<mavros_msgs::State>(pubTopicHeadConfig["pubMavrosHead"].asString() + std::to_string(i) + "/mavros/state", 10));
		}
		std::cout << "inistialize succuess! The pub number is：" << pubPoseVec.size() << std::endl;
	}


void ClientHelper::pubHelper(vector<DroneState>& stateMsgs) {
	Json::Value swarmMsg;
	for (int i = 0; i < stateMsgs.size(); ++i) {
		swarmMsg["swarmMsg"].append(stateMsgs[i].bridgeMessage);
		int dn = stateMsgs[i].DroneNumber;
		geometry_msgs::PoseStamped pose;
		pose.header.stamp = ros::Time::now();
		pose.pose.position.x = stateMsgs[i].position[0];
		pose.pose.position.y = stateMsgs[i].position[1];
		pose.pose.position.z = stateMsgs[i].position[2];

		geometry_msgs::TwistStamped vel;
		vel.header.stamp = ros::Time::now();
		vel.twist.linear.x = stateMsgs[i].velocity[0];
		vel.twist.linear.y = stateMsgs[i].velocity[1];
		vel.twist.linear.z = stateMsgs[i].velocity[2];
		mavros_msgs::State state;
		state.mode = stateMsgs[i].mode;

		pubPoseVec[dn].publish(pose);
		pubVelocityVec[dn].publish(vel);
		pubStateVec[dn].publish(state);
	}

	Json::FastWriter fw;
	std_msgs::String sm;
	sm.data = fw.write(swarmMsg);
	pubSwarmMsg.publish(sm);
	return;
}

void ClientHelper::pubHelper(ControlMsg& controlMsg) {
	mavros_msgs::SetMode mode;
	if(controlMsg.mode == "LANDING" && emergencyLandingConfig["landingMode"].asString() == "LANDING"){
		std::cout<<"\033[33m 紧急降落 \033[0m"<<std::endl;
		mode.request.custom_mode = "OFFBOARD";
		pubControlMode.call(mode);
		geometry_msgs::Vector3Stamped accel;
		accel.header.stamp = ros::Time::now();
		accel.vector.z = emergencyLandingConfig["thrust"].asDouble();
		std::cout<<accel.vector.z<<std::endl;
		pubAcc.publish(accel);
		return;
	}
	else if(controlMsg.mode == "LANDING" && emergencyLandingConfig["landingMode"].asString() == "AUTO.RTL" ){
		std::cout<<"\033[33m 自动返航\033[0m"<<std::endl;
		mode.request.custom_mode = "AUTO.RTL";
		pubControlMode.call(mode);
		return;
	}

	geometry_msgs::PoseStamped controlRos;
	controlRos.header.stamp = ros::Time::now();
	controlRos.pose.position.x = controlMsg.position[0] - originPositionVec[drone_number][0];
	controlRos.pose.position.y = controlMsg.position[1] - originPositionVec[drone_number][1];
	controlRos.pose.position.z = controlMsg.position[2] - originPositionVec[drone_number][2]; 
	controlRos.pose.orientation.x = originPositionVec[drone_number][3];
	controlRos.pose.orientation.y = originPositionVec[drone_number][4];
	controlRos.pose.orientation.z = originPositionVec[drone_number][5];
	controlRos.pose.orientation.w = originPositionVec[drone_number][6]; 

	pubControlPositon.publish(controlRos);

	if(controlMsg.mode == "OFFBOARD"){
		std::cout<<"\033[33m 定点飞行 \033[0m"<<std::endl;
		std::cout<<"\033[33m Flying to "<< controlMsg <<"\033[0m"<<std::endl;
		mode.request.custom_mode = "OFFBOARD";
		mavros_msgs::CommandBool armCmd;
		if(!subMode.armed){
			armCmd.request.value = true;
			pubArmControl.call(armCmd);
		}
	}
	else if(controlMsg.mode == "MANUAL"){
		std::cout<<"\033[33m MANUAL \033[0m"<<std::endl;
		mode.request.custom_mode = "MANUAL";
	}
	pubControlMode.call(mode);

	return;
}

int readConfig()
{
	char packagePath[256];
	char cmd[26] = "rospack find bridgeClient";
	FILE *fp = popen(cmd, "r");
	fgets(packagePath, 256, fp);
	pclose(fp);

	std::string configFilePath = std::string(packagePath).substr(0, std::string(packagePath).length() - 1) + "/../../config.txt";
	std::ifstream configFile(configFilePath);
	if(!configFile){
		std::cout << configFilePath <<std::endl;
		std::cout << "无法找到集群参数配置文件！" << std::endl;
		return 0;
	}
	std::ostringstream configOrigin;
	configOrigin << configFile.rdbuf();
	Json::Reader reader;
	Json::Value config;
	if (reader.parse(configOrigin.str(), config))
	{
		Json::Value swarmConfig = config["swarmConfig"];
		max_swarm_number = swarmConfig["swarmSize"].asInt();
		originPositionVec.resize(max_swarm_number);
		server_ip = swarmConfig["serverIp"].asString();
		listeningPort = swarmConfig["listeningPort"].asInt();
		Json::Value originPositionConfig = config["originPositionConfig"];
		
		for(int i=0;i<max_swarm_number;++i){
			Json::Value data = originPositionConfig[i];
			originPositionVec[i] = vector<double>{data["positionX"].asDouble(),
												data["positionY"].asDouble(),
												data["positionZ"].asDouble(),
												data["orientationX"].asDouble(),
												data["orientationY"].asDouble(),
												data["orientationZ"].asDouble(),
												data["orientationW"].asDouble()};
		}
		subTopicConfig = config["subTopicConfig"];
		pubTopicHeadConfig = config["pubTopicHeadConfig"];
		pubControlConfig = config["pubControlConfig"];
		emergencyLandingConfig = config["emergencyLandingConfig"];
	}
	else {
		std::cout << "集群参数配置文件json格式错误..." << std::endl;
		return 0;
	}
	configFile.close();
	return 1;
}


int main(int argc, char **argv) {	
	std::cout<<"请输入当前无人机编号(从0开始): ";
	std::cin>>drone_number;
	std::cout << "The Drone number is: " << drone_number << std::endl;
	if(!readConfig()){
		return 0;
	}
	if(drone_number<0 || max_swarm_number<=0){
		std::cout<<"drone_number must greater than or equal to zero, and max_swarm_number must greater than zero..."<<std::endl;
		return -2;
	}
	if(drone_number>=max_swarm_number){
		std::cout<<"drone_number must smaller than max_swarm_number, the index begins from 0..."<<std::endl;
		return -3;
	}
	std::cout<<"droneNumber is :" << drone_number <<std::endl;
	std::cout<<"max swarms number is :" <<max_swarm_number<<std::endl;
	std::cout<<"server's ip address is :" <<server_ip<<std::endl;
	std::cout<<server_ip<<std::endl;
	std::cout<<std::endl;

	ros::init(argc, argv, "droneClient"+std::to_string(drone_number));
	ros::NodeHandle n;
	ros::Rate rate(10);
	Conn conn(server_ip.c_str(), listeningPort);
	if (conn.fd == -1)
	{
		perror("listen socket initialize fail...");
		exit(0);
	}
	Client ct(conn.fd, &conn.addr);
	DroneState* myMsg = new DroneState(drone_number);
	ClientHelper ch(n, myMsg, max_swarm_number);
	while(ros::ok()){
		myMsg->clear();
		ros::spinOnce();
		ct.sendMsg(myMsg);
		int len;
		vector<DroneState> swarmsStateMsgs;
		ControlMsg controlMsg;
		// len = ct.recvMsg(myMsg, swarmsStateMsgs);
		len = ct.recvMsg(myMsg, swarmsStateMsgs, controlMsg);
		// 有控制点信息
		if(controlMsg.droneNumber != -1){
			ch.pubHelper(controlMsg);
		}
		// else{
		// 	ch.pubHelper(controlMsg);
		// }
		ch.pubHelper(swarmsStateMsgs);

		
		if (len > 0){
			rate.sleep();
		}
		else if (len == 0)
		{
			printf("server disconneted...\n");
			break;
		}
		else
		{
			perror("read");
			break;
		}
	}
	close(conn.fd);
	return 0;
}
